#ifndef VEC3F_H
#define VEC3F_H

#include <math.h>
#include <cstdlib>

#define		TWO_PI			6.28318530f
#define		HALF_PI			1.57079633f
#define		dProd(a,b)		( (a).x*(b).x + (a).y*(b).y + (a).z*(b).z )
#define		mag(a)			( dProd(a,a) )
#define		len(a)			( sqrt(mag(a)) )
#define		sqr(a)			( (a)*(a) )
#define		less(a,b)		( a.x<b.x && a.y<b.y && a.z<b.z )
#define		more(a,b)		( a.x>b.x && a.y>b.y && a.z>b.z )
#define		nearZero(a)		( (a) > -0.05f && (a) < 0.05f )
#define		nearZeroVec(a)	(nearZero(a.x) && nearZero(a.y) && nearZero(a.z))
#define		inTri(a,b,c)	( acos(dProd(a,b))+acos(dProd(a,c))+acos(dProd(b,c)) ) 

struct vec3f { 
	public: 
		float x, y, z;
		vec3f() {}
		vec3f(float a, float b, float c) { x=a; y=b; z=c; }
		vec3f(float a[3]) { x=a[0]; y=a[1]; z=a[2]; }
};

inline vec3f newVec(float a, float b, float c) {
	vec3f newV; newV.x = a; newV.y = b; newV.z = c;
	return newV;
}

inline bool sameVec(vec3f &a, vec3f &b) {
	return ( a.x == b.x && a.y == b.y && a.z == b.z );
}

inline vec3f addVec(const vec3f &a, const vec3f &b) {
	return vec3f( a.x+b.x, a.y+b.y, a.z+b.z );
}

inline vec3f subVec(const vec3f &a, const vec3f &b) {
	return vec3f( a.x-b.x, a.y-b.y, a.z-b.z );
}

inline vec3f multVec(vec3f &a, vec3f &b) {
	return vec3f( a.x*b.x, a.y*b.y, a.z*b.z );
}

inline vec3f scaleVec(vec3f &a, float b) {
	return vec3f( a.x*b, a.y*b, a.z*b );
}

inline vec3f normalize(vec3f &a) {
	float length = len(a);
	if ( length == 0 ) { return a; }
	length = (1.0f / length);
	return scaleVec(a, length);
}

inline vec3f normalizeAtLength(vec3f &a, float length) {
	if ( length == 0 ) { return a; }
	length = (1.0f / length);
	return scaleVec(a, length);
}

inline vec3f normalizeToLength(vec3f &a, float normLength) {
	float length = len(a);
	if ( length == 0 ) { return a; }
	length = (normLength / length);
	return scaleVec(a, length);
}

inline vec3f cProd(const vec3f &a, const vec3f &b) {	
	return vec3f( ((a.y*b.z) - (a.z*b.y)),
				  ((a.z*b.x) - (a.x*b.z)),
	 			  ((a.x*b.y) - (a.y*b.x)) );
}

inline bool onLineSeg(vec3f &p, vec3f &a, vec3f &b, vec3f &v) {

	vec3f c = subVec(b, a);

	// check cross product step by step for speed
	if ( !nearZero( (c.y*v.z)-(c.z*v.y) ) ) { return false; }
	if ( !nearZero( (c.z*v.x)-(c.x*v.z) ) ) { return false; }
	if ( !nearZero( (c.x*v.y)-(c.y*v.x) ) ) { return false; }
	
	float dist1 = len(subVec(p,a)) + len(subVec(p,b));
	float dist2 = len(subVec(a,b));
	return nearZero(dist1 - dist2);
}

inline bool pointInTri(vec3f p, vec3f v0, vec3f v1, vec3f v2, vec3f fn, float dist, float rad, float epsilon) {
	
	float push = dist < rad ? dist : rad;
	p = subVec(p, scaleVec(fn,push));

    vec3f vec0 = subVec(p,v0);
    vec3f vec1 = subVec(p,v1);
    vec3f vec2 = subVec(p,v2);

	vec0 = normalize(vec0);
	vec1 = normalize(vec1);
	vec2 = normalize(vec2);

    float angle =	acos(dProd(vec0,vec1))+ 
					acos(dProd(vec1,vec2))+ 
					acos(dProd(vec2,vec0));
 
	if ( abs( angle - TWO_PI ) < epsilon ) { 
		return true; }

	if ( onLineSeg(p, v0, v1, vec0) || 
		 onLineSeg(p, v1, v2, vec1) ||
		 onLineSeg(p, v2, v0, vec2) ) { 
		return true;
	}

	return false;
}

inline bool sameSide(vec3f p1, vec3f p2, vec3f a, vec3f b) {
	vec3f cp1 = cProd(subVec(b,a),subVec(p1,a));
	vec3f cp2 = cProd(subVec(b,a),subVec(p2,a));
	if ( dProd(cp1,cp2) >= 0 ) { return true; }
	return false;
}

inline bool pointInTriCP(vec3f p, vec3f a, vec3f b, vec3f c) {
	if ( sameSide(p,a,b,c) && sameSide(p,b,a,c) && sameSide(p,c,a,b) ) {
		return true;
	}
	return false;
}

inline vec3f randVec() {	
	float halfMax = RAND_MAX >> 1;
	vec3f r = vec3f(rand()-halfMax, rand()-halfMax, rand()-halfMax);
	return normalize(r);
}

#endif